// Include files.
#include "../Internal/NullDeleter.hpp"
#include "EncoderMotorController.hpp"

using namespace boost;

/**
 *  Constructor for EncoderMotorController.
 *  
 *  @param motor A shared pointer to the SpeedController.
 *  @param encoder A shared pointer to the Encoder.
 *  @param period The desired time between PID loops in seconds
 */
EncoderMotorController::EncoderMotorController(shared_ptr<SpeedController> motor, shared_ptr<Encoder> encoder, float period) :
	m_motor(motor),
	m_encoder(encoder),
	m_period(period),
	m_notifier(ResetTimer, this),
	m_pid(0, 0, 0, this, m_motor.get(), period),
	m_speed(0),
	m_offset(0),
	m_controlMode(PercentVBus),
	m_sourceFunction(GetVBus)
{
	Initialize();
}

/**
 *  Constructor for EncoderMotorController.
 *  
 *  @param motor A shared pointer to the SpeedController.
 *  @param encoderChannelA The first channel number of the encoder.
 *  @param encoderChannelB The second channel number of the encoder.
 *  @param encoderReverseDirection If the encoder is reversed.
 *  @param period The desired time between PID loops in seconds
 */
EncoderMotorController::EncoderMotorController(shared_ptr<SpeedController> motor, UINT32 encoderChannelA, UINT32 encoderChannelB, bool encoderReverseDirection, float period) :
	m_motor(motor),
	m_encoder(new Encoder(encoderChannelA, encoderChannelB, encoderReverseDirection)),
	m_period(period),
	m_notifier(ResetTimer, this),
	m_pid(0, 0, 0, this, m_motor.get(), period),
	m_speed(0),
	m_offset(0),
	m_controlMode(PercentVBus),
	m_sourceFunction(GetVBus)
{
	Initialize();
}

/**
 *  Constructor for EncoderMotorController.
 *  
 *  @param motor A pointer to the SpeedController.
 *  @param encoder A pointer to the Encoder.
 *  @param period The desired time between PID loops in seconds
 */
EncoderMotorController::EncoderMotorController(SpeedController *motor, Encoder *encoder, float period) :
	m_motor(shared_ptr<SpeedController>(motor, NullDeleter())),
	m_encoder(shared_ptr<Encoder>(encoder, NullDeleter())),
	m_period(period),
	m_notifier(ResetTimer, this),
	m_pid(0, 0, 0, this, m_motor.get(), period),
	m_speed(0),
	m_offset(0),
	m_controlMode(PercentVBus),
	m_sourceFunction(GetVBus)
{
	Initialize();
}

/**
 *  Constructor for EncoderMotorController.
 *  
 *  @param motor A pointer to the SpeedController.
 *  @param encoderChannelA The first channel number of the encoder.
 *  @param encoderChannelB The second channel number of the encoder.
 *  @param encoderReverseDirection If the encoder is reversed.
 *  @param period The desired time between PID loops in seconds
 */
EncoderMotorController::EncoderMotorController(SpeedController *motor, UINT32 encoderChannelA, UINT32 encoderChannelB, bool encoderReverseDirection, float period) :
	m_motor(shared_ptr<SpeedController>(motor, NullDeleter())),
	m_encoder(new Encoder(encoderChannelA, encoderChannelB, encoderReverseDirection)),
	m_period(period),
	m_notifier(ResetTimer, this),
	m_pid(0, 0, 0, this, m_motor.get(), period),
	m_speed(0),
	m_offset(0),
	m_controlMode(PercentVBus),
	m_sourceFunction(GetVBus)
{
	Initialize();
}

/**
 *  Constructor for EncoderMotorController.
 *  
 *  @param motor A reference to the SpeedController.
 *  @param encoder A reference to the Encoder.
 *  @param period The desired time between PID loops in seconds
 */
EncoderMotorController::EncoderMotorController(SpeedController &motor, Encoder &encoder, float period) :
	m_motor(shared_ptr<SpeedController>(&motor, NullDeleter())),
	m_encoder(shared_ptr<Encoder>(&encoder, NullDeleter())),
	m_period(period),
	m_notifier(ResetTimer, this),
	m_pid(0, 0, 0, this, m_motor.get(), period),
	m_speed(0),
	m_offset(0),
	m_controlMode(PercentVBus),
	m_sourceFunction(GetVBus)
{
	Initialize();
}

/**
 *  Constructor for EncoderMotorController.
 *  
 *  @param motor A reference to the SpeedController.
 *  @param encoderChannelA The first channel number of the encoder.
 *  @param encoderChannelB The second channel number of the encoder.
 *  @param encoderReverseDirection If the encoder is reversed.
 *  @param period The desired time between PID loops in seconds
 */
EncoderMotorController::EncoderMotorController(SpeedController &motor, UINT32 encoderChannelA, UINT32 encoderChannelB, bool encoderReverseDirection, float period) :
	m_motor(shared_ptr<SpeedController>(&motor, NullDeleter())),
	m_encoder(new Encoder(encoderChannelA, encoderChannelB, encoderReverseDirection)),
	m_period(period),
	m_notifier(ResetTimer, this),
	m_pid(0, 0, 0, this, m_motor.get(), period),
	m_speed(0),
	m_offset(0),
	m_controlMode(PercentVBus),
	m_sourceFunction(GetVBus)
{
	Initialize();
}

/**
 *  Set the value of the motor.
 *  
 *  What it sets depends on the currently set ControlMode.
 *  
 *  @param value The target value.
 *  @param syncGroup Not applicable for this derived class. (The default value of this parameter will work fine.)
 */
void EncoderMotorController::Set(float value, UINT8 syncGroup)
{
	if (m_controlMode == PercentVBus)
		m_motor->Set(value, syncGroup);
	else
		m_pid.SetSetpoint(value);
}

/**
 *  Gets the current value of the motor.
 *  
 *  What it gets depends on the currently set ControlMode.
 *  
 *  @return The current value of the motor.
 */
float EncoderMotorController::Get()
{
	return m_sourceFunction(this);
}

/**
 *  Gets the percentage of voltage sent to the motor where 1 represents 100%.
 *  
 *  @return The percentage of voltage sent to the motor where 1 represents 100%.
 */
double EncoderMotorController::GetVBus()
{
	return m_motor->Get();
}

/**
 *  Get the current speed that the motor is running in rotations per second.
 *  
 *  @return The current speed of the motor in rotations per second.
 */
double EncoderMotorController::GetSpeed()
{
	/*double time = m_timer.Get();
	
	if (time != 0)
		m_speed = GetPosition() / time;
	
	return m_speed;
	*/
	return m_encoder->GetRate();
}

/**
 *  Get the current position in units of rotations that the motor has reached since motor was enabled.
 *  
 *  @return The current position of the motor. 
 */
double EncoderMotorController::GetPosition()
{
	return m_encoder->GetDistance() + m_offset;
}

/**
 *  Enable the motor and the internal PID loop.
 *  
 *  If the parameter initialEncoderPosition is passed, the current position should be treated as initialEncoderPosition. 
 *  
 *  @param initialEncoderPosition The initial position of the motor.
 */
void EncoderMotorController::Enable(double initialEncoderPosition)
{
	m_offset = initialEncoderPosition;
	m_encoder->Reset();
	
	if (m_controlMode == PercentVBus)
		m_pid.Disable();
	else
		m_pid.Enable();
	
	m_notifier.StartPeriodic(m_period);
}

/**
 *  Disable the motor and the internal PID loop.
 */
void EncoderMotorController::Disable()
{
	m_pid.Disable();
	m_motor->Disable();
	m_notifier.Stop();
}

/**
 *  Get the current ControlMode that the motor is in.
 *  
 *  @return The current ControlMode. 
 */
PidMotorController::ControlMode EncoderMotorController::GetControlMode()
{
	return m_controlMode;
}

/**
 *  Set the ControlMode immediately.
 *  
 *  Warning: Any ControlMode passed other than PercentVBus, Speed, or Position will throw the controlMode parameter as an exception.
 *  
 *  @param mode The desired ControlMode. 
 */
void EncoderMotorController::SetControlMode(PidMotorController::ControlMode mode)
{
	m_controlMode = mode;
	
	switch (m_controlMode)
	{
		case PercentVBus:
			m_sourceFunction = GetVBus;
			m_pid.Disable();
			break;
		case Speed:
			m_sourceFunction = GetSpeed;
			m_pid.Enable();
			break;
		case Position:
			m_sourceFunction = GetPosition;
			m_pid.Enable();
			break;
		default:
			throw m_controlMode;
	}
}

/**
 *  Set the number of encoder codes per revolution.
 *  
 *  Specify the number of encoder "codes" it takes to complete a revolution.
 *  
 *  @param codesPerRevolution The number of encoder codes per revolution.
 */
void EncoderMotorController::SetEncodersCodesPerRevolution(UINT16 codesPerRevolution)
{
	double distancePerPulse;
	
	if (codesPerRevolution == 0)
		throw codesPerRevolution;
	
	distancePerPulse = 1.0 / double(codesPerRevolution);
	
	m_encoder->SetDistancePerPulse(distancePerPulse);
}

/**
 *  Set the P, I, and D values.
 *  
 *  @param p The P value.
 *  @param i The I value.
 *  @param d The D value.
 */
void EncoderMotorController::SetPid(double p, double i, double d)
{
	m_pid.SetPID(p, i, d);
}

/**
 *  Get the current P value
 *  
 *  @return The current P value.
 */
double EncoderMotorController::GetP()
{
	return m_pid.GetP();
}

/**
 *  Get the current I value
 *  
 *  @return The current I value.
 */
double EncoderMotorController::GetI()
{
	return m_pid.GetI();
}

/**
 *  Get the current D value
 *  
 *  @return The current D value.
 */
double EncoderMotorController::GetD()
{
	return m_pid.GetD();
}

/**
 *  Initializes the PID loop controller object's table.
 *  
 *  Unless you use NetworkTables, don't worry about this function.
 *  
 *  @param subtable The table to put values in.
 */
void EncoderMotorController::InitTable(ITable *subtable)
{
	m_pid.InitTable(subtable);
}

/**
 *  Get the table associated the PID loop controller object's table.
 *  
 *  Unless you use NetworkTables, don't worry about this function.
 *  
 *  @return The table associated the PID loop controller object's table.
 */
ITable *EncoderMotorController::GetTable()
{
	return static_cast<LiveWindowSendable&>(m_pid).GetTable();
}

/**
 *  Get the string representing the named data type of the PID loop controller object.
 *  
 *  Unless you use NetworkTables, don't worry about this function.
 *  
 *  @return The string representing the named data type of the PID loop controller object.
 */
std::string EncoderMotorController::GetSmartDashboardType()
{
	return static_cast<LiveWindowSendable&>(m_pid).GetSmartDashboardType();
}

/**
 *  Update the table associated the PID loop controller object's table.
 *  
 *  Unless you use NetworkTables, don't worry about this function.
 */
void EncoderMotorController::UpdateTable()
{
	static_cast<LiveWindowSendable&>(m_pid).UpdateTable();
}

/**
 *  Start Live Window mode for the PID loop controller object.
 *  
 *  Unless you use LiveWindow, don't worry about this function.
 */
void EncoderMotorController::StartLiveWindowMode()
{
	static_cast<LiveWindowSendable&>(m_pid).StartLiveWindowMode();
}

/**
 *  Stop Live Window mode for the PID loop controller object.
 *  
 *  Unless you use LiveWindow, don't worry about this function.
 */
void EncoderMotorController::StopLiveWindowMode()
{
	static_cast<LiveWindowSendable&>(m_pid).StopLiveWindowMode();
}

/**
 *  Gets the percentage of voltage sent to the motor where 1 represents 100%.
 *  
 *  @param motor The EncoderMotorController to read.
 *  @return The percentage of voltage sent to the motor where 1 represents 100%.
 */
double EncoderMotorController::GetVBus(EncoderMotorController *motor)
{
	return motor->GetVBus();
}

/**
 *  Get the current speed that the motor is running in rotations per second.
 *  
 *  @param motor The EncoderMotorController to read.
 *  @return The current speed of the motor in rotations per second.
 */
double EncoderMotorController::GetSpeed(EncoderMotorController *motor)
{
	return motor->GetSpeed();
}

/**
 *  Get the current position in units of rotations that the motor has reached since motor was enabled.
 *  
 *  @param motor The EncoderMotorController to read.
 *  @return The current position of the motor. 
 */
double EncoderMotorController::GetPosition(EncoderMotorController *motor)
{
	return motor->GetPosition();
}

/**
 *  Reset the timer for the EncoderMotorController.
 *  
 *  If someone gets rid of m_timer and m_notifier (neither of them are necessary), this function can be deleted.
 *  
 *  @param pseudoCanJaguar A void pointer to the EncoderMotorController.
 */
void EncoderMotorController::ResetTimer(void *pseudoCanJaguar)
{
	/*
	 * Na na na na, Sketch-man!
	 * Ask Sam if you want to know why templated pointers are better than void pointers.
	 * No. I will not.
	 */
	
	static_cast<EncoderMotorController*>(pseudoCanJaguar)->m_timer.Reset();
	static_cast<EncoderMotorController*>(pseudoCanJaguar)->m_timer.Start();
}

/**
 *  Initialize the EncoderMotorController by starting the encoder and resetting the unused timer.
 */
void EncoderMotorController::Initialize()
{
	m_encoder->Start();
	m_timer.Stop();
	m_timer.Reset();
}
